#include "nezha_can_adapter.h"
#include "ep30.h"
#include "sensor_utils.hpp"
#include <linux/can.h>
#include <linux/can/raw.h>

using namespace sensor::radar;
using namespace sensor::utils;

NezhaCanAdapter::NezhaCanAdapter(std::unordered_map<std::string, std::shared_ptr<CanTransreceiver>> devs)
    : CanAdapter(devs, "Nezha")
{
    using namespace std::placeholders;
    message_unpack_table_ = {

        { EP30_ESC_FR07_291_FRAME_ID, std::bind(&NezhaCanAdapter::onReceiveEP30291Info, this, _1, _2, _3) },
        { EP30_VCU_FR05_223_FRAME_ID, std::bind(&NezhaCanAdapter::onReceiveEP30223Info, this, _1, _2, _3) },
        { EP30_APA_FR03_27_B_FRAME_ID, std::bind(&NezhaCanAdapter::onReceiveEP3027BInfo, this, _1, _2, _3) },
        { EP30_GW_EDR2_2_FF_FRAME_ID, std::bind(&NezhaCanAdapter::onReceiveEP302FFInfo, this, _1, _2, _3) },
    };

    std::map<uint16_t, unsigned int> canIdMap = {
        { EP30_ESC_FR07_291_FRAME_ID, CAN_EFF_MASK },
        { EP30_VCU_FR05_223_FRAME_ID, CAN_EFF_MASK },
        { EP30_APA_FR03_27_B_FRAME_ID, CAN_SFF_MASK },
        { EP30_GW_EDR2_2_FF_FRAME_ID, CAN_SFF_MASK },
    };
    can_devs_["can0"]->filter(canIdMap);
}

void NezhaCanAdapter::onReceiveCanMessage(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    RCLCPP_INFO(logger_, "zezha Rx:%s %04x %d %s", dev.c_str(), fid, buffer.readableBytes(),
        encodeToHexString(buffer.toStringPiece().as_string()).c_str());

    auto iter = message_unpack_table_.find(fid);
    if (iter == message_unpack_table_.end()) {
        // RCLCPP_WARN(logger_, "No Handle to unpack frame %04x", fid);
        return;
    }

    iter->second(dev, fid, buffer);
}
void NezhaCanAdapter::onReceiveGnssMessage(const interface::msg::Gnss::SharedPtr msg)
{
}
void NezhaCanAdapter::onReceiveInformationMessage(const interface::msg::CarInformation msg)
{
    RCLCPP_INFO(logger_, "  -----------  ZiyanCanAdapter 1 --------------- onReceiveInformationMessage start %f : %f : %f", msg.speed, msg.yawrate,msg.gearstate);
    // 手动档位
    static std::unordered_map<int, int> gearsMkMap = {
        { 0, 0xa },
        { 1, 0x1 },
        { 2, 0x2 },
        { 3, 0x3 },
        { 4, 0x4 },
        { 5, 0x5 },
        { 6, 0x6 },
        { 7, 0xb },
        { 8, 0x2 },
    };
    // 自动档位
    static std::unordered_map<int, int> gearsAutoMkMap = {
        { 0, 0x0 },
        { 1, 0x1 },
        { 2, 0x2 },
        { 3, 0x3 },
        { 4, 0x4 },
        { 5, 0x5 },
        { 6, 0x6 },
        { 7, 0x7 },
        { 8, 0x5 },
    };
    {
        // 方向盘 0xA1
        b30_001_csa2_t csa2 { 0 };
        csa2.steer_wheel_ang_sign = (msg.wheelangle >= 0) ? 1 : 0;
        csa2.steer_wheel_ang = b30_001_csa2_steer_wheel_ang_encode(std::abs(msg.wheelangle));
        // 0表示负数，1表示正数
        
        uint8_t dstp[8];
        if (int ret = b30_001_csa2_pack(dstp, &csa2, sizeof(dstp))) {
            muduo::net::Buffer buffer;
            buffer.append(dstp, ret);
            canSend("can1", B30_001_CSA2_FRAME_ID, buffer);
        }
    }
    {
        // 车速 0x137
        b30_001_esp_fd2_t espfd2 { 0 };
        espfd2.veh_spd = b30_001_esp_fd2_veh_spd_encode(msg.speed);
        espfd2.veh_spd_vld = b30_001_esp_fd2_veh_spd_vld_encode(1.0);
        uint8_t dstp[64];
        if (int ret = b30_001_esp_fd2_pack(dstp, &espfd2, sizeof(dstp))) {
            muduo::net::Buffer buffer;
            buffer.append(dstp, ret);
            canSend("can1", B30_001_ESP_FD2_FRAME_ID, buffer);
        }
    }
    {
        // 自动档位  0x12f
        b30_001_hcu_fd3_t hcudf3 { 0 };
        
        hcudf3.hcu_drv_mod = gearsAutoMkMap[msg.gearstate];
        uint8_t dstp[64];
        if (int ret = b30_001_hcu_fd3_pack(dstp, &hcudf3, sizeof(dstp))) {
            muduo::net::Buffer buffer;
            buffer.append(dstp, ret);
            canSend("can1", B30_001_HCU_FD3_FRAME_ID, buffer);
        }
    }
    {
        // 偏航角 0x245
        b30_001_abm2_t abm2 { 0 };
        abm2.veh_yaw_rate = b30_001_abm2_veh_yaw_rate_encode(msg.yawrate * M_PI / 180.0);
        uint8_t dstp[8];
        if (int ret = b30_001_abm2_pack(dstp, &abm2, sizeof(abm2))) {
            muduo::net::Buffer buffer;
            buffer.append(dstp, ret);
            canSend("can1", B30_001_ABM2_FRAME_ID, buffer);
        }
    }
    RCLCPP_INFO(logger_, "  -----------  ZiyanCanAdapter 2 --------------- onReceiveInformationMessage start %f : %f : %d : %f", msg.speed, msg.yawrate,gearsAutoMkMap[msg.gearstate],msg.wheelangle);

}

void NezhaCanAdapter::onReceiveEP30291Info(const std::string&, int fid, muduo::net::Buffer& buffer)
{
    struct ep30_esc_fr07_291_t track;
    auto piece = buffer.toStringPiece();
    int retv = ep30_esc_fr07_291_unpack(&track, (const uint8_t*)(piece.data()), piece.size());
    car_information_.speed = track.esc7_vehicle_spd * 0.03125;
    car_information_.yawrate = track.esc7_vehicle_dyn_yaw_rate * 0.01;
    RCLCPP_INFO(logger_, "[%04x]ep30_esc_fr07_291_unpack return %d , speed : %lf yawrate : %lf", fid, retv, car_information_.speed, car_information_.yawrate);
    body_information_pubs_->publish(car_information_);
}
void NezhaCanAdapter::onReceiveEP30223Info(const std::string&, int fid, muduo::net::Buffer& buffer)
{
    // // 档位匹配
    static std::unordered_map<int, int> gears223Map = {
        { 0x5, 0 },
        { 0x6, 0 },
        { 0x8, 8 },
        { 0xA, 0 },
        { 0XB, 8 },
        { 0xC, 0 },
        { 0xD, 7 },
        { 0xE, 0 },
        { 0xF, 0 },
    };
    struct ep30_vcu_fr05_223_t track;
    auto piece = buffer.toStringPiece();
    int retv = ep30_vcu_fr05_223_unpack(&track, (const uint8_t*)(piece.data()), piece.size());
    car_information_.gearstate = gears223Map[track.vcu5_gearbox_position_display];
    body_information_pubs_->publish(car_information_);
    RCLCPP_INFO(logger_, "[%04x]ep30_vcu_fr05_223_unpack return %d , gearbox : %d", fid, retv, car_information_.gearstate);
}
void NezhaCanAdapter::onReceiveEP3027BInfo(const std::string&, int fid, muduo::net::Buffer& buffer)
{
    // 档位匹配
    static std::unordered_map<int, int> gearsMap = {
        { 0, 0 },
        { 0x1, 0 },
        { 0x2, 7 },
        { 0x3, 0 },
        { 0X4, 8 },
    };
    struct ep30_apa_fr03_27_b_t track;
    auto piece = buffer.toStringPiece();
    int retv = ep30_apa_fr03_27_b_unpack(&track, (const uint8_t*)(piece.data()), piece.size());
    if (track.apa_gear_pos_req_validity) {
        car_information_.gearstate = gearsMap[track.apa_gear_pos_req];
    }
    body_information_pubs_->publish(car_information_);
    RCLCPP_INFO(logger_, "[%04x]ep30_apa_fr03_27_b_unpack return %d, gears : %d", fid, retv, track.apa_gear_pos_req);
}

void NezhaCanAdapter::onReceiveEP302FFInfo(const std::string&, int fid, muduo::net::Buffer& buffer)
{
    // 方向盘角度
    struct ep30_gw_edr2_2_ff_t track;
    auto piece = buffer.toStringPiece();
    int retv = ep30_gw_edr2_2_ff_unpack(&track, (const uint8_t*)(piece.data()), piece.size());//(msg.wheelangle >= 0) ? 1 : 0
    int16_t valid = track.eps_steering_angle_valid == 1 ? 1 : -1;
    car_information_.wheelangle = ep30_gw_edr2_2_ff_eps_steering_angle_decode(track.eps_steering_angle);

    body_information_pubs_->publish(car_information_);
    RCLCPP_INFO(logger_, "[%04x]ep30_gw_edr2_2_ff_unpack return %d, gears : %f", fid, retv, car_information_.wheelangle);
}
